#include "../_includes.h"
// //挖角落粽子，怼进网；吸边杆粽，怼进网；吸预装粽，怼进网；
void auto_right_5() {
    ActionUnit::set_all_units_motor_stop_status_on_working;
    gyro_reset();
    stable_go(-1000,100,500,2000);
}
// void auto_right_5() {
//   dig.act_for_enc_absolute(enc_dig_collect_triball, 100, 800,
//                            Block::yes); // 挖角落
//   dig.set_voltage(-35);
//   intake.set_voltage(100);
//   chassis.stable_forward_dist_relative(-1440, 100, 2000,
//                                        Block::yes); //退到网正面前
//   chassis.fast_forward_dist_relative(50, 80, 300, Block::yes); //接粽子
//   wait(200);
//   dig.act_for_enc_absolute(enc_dig_upright - 80, 100, 1500,
//                            Block::no);                     //
//   turn_pid_adjust_agl_absolute(-45, 100, 700, Block::yes); //转向正网
//   intake.set_voltage(-100);
//   wait(100);
//   chassis.stable_forward_dist_relative(750, 100, 1500,
//                                        Block::yes); //怼进角落粽子
//   chassis.stable_forward_dist_relative(-150, 100, 800,
//                                        Block::yes);        //稍微后退
//   turn_pid_adjust_agl_absolute(122, 100, 800, Block::yes); //转向边粽
//   intake.set_voltage(100);
//   chassis.stable_forward_dist_relative(520, 100, 1500,
//                                        Block::yes); //走向边粽
//   dig.act_for_enc_absolute(enc_dig_collect_triball, 100, 800,
//                            Block::no); // 护住
//   chassis.stable_forward_dist_relative(-450, 100, 1500,
//                                        Block::yes);            //拉回边粽
//   chassis.fast_forward_dist_relative(50, 80, 300, Block::yes); //接粽子
//   wait(200);
//   dig.act_for_enc_absolute(enc_dig_upright - 80, 100, 1500,
//                            Block::no);                     //立起来
//   turn_pid_adjust_agl_absolute(-48, 100, 800, Block::yes); //转向正网
//   intake.set_voltage(-100);
//   wait(100);
//   intake.stop(coast);

//   chassis.fast_forward_dist_relative(400, 100, 1000,
//                                      Block::yes); //边粽怼进去
//   chassis.stable_forward_dist_relative(-200, 100, 800,
//                                        Block::yes);       //退网
//   turn_pid_adjust_agl_absolute(40, 100, 800, Block::yes); //转向颜色粽子
//   intake.set_voltage(100);
//   chassis.stable_forward_dist_relative(920, 100, 1800,
//                                        Block::yes); //走向颜色粽子
//   dig.act_for_enc_absolute(enc_dig_collect_triball, 100, 800,
//                            Block::no); // 护住
//   chassis.stable_forward_dist_relative(-1020, 100, 1800,
//                                        Block::yes); //退到正网
//   dig.act_for_enc_absolute(enc_dig_upright - 80, 100, 1500,
//                            Block::no);                         //立起来
//   chassis.fast_forward_dist_relative(50, 80, 300, Block::yes); //接粽子
//   wait(200);
//   turn_pid_adjust_agl_absolute(-45, 100, 700, Block::yes); //转向正网
//   intake.set_voltage(-100);
//   wait(100);
//   intake.stop(coast);
//   chassis.fast_forward_dist_relative(450, 100, 800,
//                                      Block::yes); //边粽怼进去
//   chassis.fast_forward_dist_relative(-200, 100, 800,
//                                      Block::yes); //退网
// }